#pragma once

#include <QVector3D>
#include <QObject>
#include <QByteArray>
#include <nlohmann/json.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/quaternion.hpp>


using nlohmann::json;


namespace rviz_custom_plugins 
{


namespace models 
{


class Pose
{

public:
    Pose() = default;

    ~Pose() = default;

    int id = 0;

    geometry_msgs::msg::Point position;
    geometry_msgs::msg::Quaternion orientation;
};


inline void to_json(json& j, const Pose& pose) {
    // 计算 yaw
    tf2::Quaternion q(pose.orientation.x, pose.orientation.y,
        pose.orientation.z, pose.orientation.w);
    double roll, pitch, yaw;
    tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);

    j = json{
        {"id", pose.id},
        {"pos", json::array({pose.position.x, pose.position.y, pose.position.z})},
        {"yaw", yaw}
    };
}


inline void from_json(const json&j, Pose& pose)
{
    pose.id = j.value("id", 0);

    auto position = j.at("pos");
    pose.position.x = position[0];
    pose.position.y = position[1];
    pose.position.z = position[2];
    // double yaw = j.at("yaw").get<double>();
    double yaw = j.value("yaw", 0.0);
    tf2::Quaternion q;
    q.setRPY(0.0, 0.0, yaw);  // roll/pitch = 0
    pose.orientation.x = q.x();
    pose.orientation.y = q.y();
    pose.orientation.z = q.z();
    pose.orientation.w = q.w();
    
}

}


}


Q_DECLARE_METATYPE(rviz_custom_plugins::models::Pose)